#ifndef _PI_Contrller_H__
#define _PI_Contrller_H__

#include "os/types.h" // IWYU pragma: export

typedef struct {
	float  kp;
	float  ki;
	float  integral;
	float  max;
	float  min;
	float  ts;
}pi_t;

__STATIC_INLINE void pi_set_params(pi_t *pi, float kp, float ki) {
	pi->kp = kp;
	pi->ki = ki;
}

__STATIC_INLINE void pi_set_saturate(pi_t *pi, float max, float min) {
	pi->max = max;
	pi->min = min;
}

__STATIC_INLINE void pi_reset(pi_t *pi, float init) {
	pi->integral = (init);
}

__STATIC_INLINE float pi_run(pi_t *pi, float err, float ff) {
	float kp_err = err * pi->kp;
	float integral_term = err * pi->ts * pi->ki;
	float out, out_presat;

	pi->integral += integral_term;
	out = pi->integral + kp_err + ff;
	out_presat = out;
	if (out > pi->max) {
		out = pi->max;
		if (kp_err >= pi->max) {
			pi->integral = 0;
		}else {
			pi->integral += (out - out_presat);
		}
	}else if (out < pi->min) {
		out = pi->min;
		if (kp_err <= pi->min) {
			pi->integral = 0;
		}else {
			pi->integral += (out - out_presat);
		}
	}
	return out;
}

#endif /* _PI_Contrller_H__ */
